package com.step.math;

import android.util.FloatMath;

import com.step.Step.Dimension;
import com.step.utils.MathUtils;

public class Vector {	
	float[] values;
	
	final int len;
	
	public Vector(float x, float y){
		len = 2;
		values = new float[len];
		set(x, y);
	}
	public Vector(float x, float y, float z){
		len = 3;
		values = new float[len];
		set(x, y, z);
	}
	
	public Vector(Dimension dimension){		
		len = dimension == Dimension.d2d ? 2 : 3;
		values = new float[len];
	}
	
	public boolean is2D(){
		return len == 2;
	}
	
	public boolean is3D(){
		return len == 3;
	}
	
	// 判断是否相同维度(同为2D或同为3D)
	boolean dimensionEquals(Vector vector){
		return len == vector.len;
	}
	
	public float getX(){
		return values[0];
	}
	
	public float getY(){
		return values[1];
	}
	
	public float getZ(){
		return values.length == 2 ? 0 : values[2];
	}
	
	public void setX(float x){
		values[0] = x;
	}
	
	public void setY(float y){
		values[1] = y;
	}
	
	public void setZ(float z){
		values[2] = z;
	}
	
	public Vector set(float x, float y, float z){
		set(x, y);
		if(len == 3){
			values[2] = z;
		}
		return this;
	}
	
	public Vector set(float x, float y){
		values[0] = x;
		values[1] = y;
		return this;
	}
	
	public Vector set(Vector vector){
		if(!dimensionEquals(vector)) throw new RuntimeException("Vector:维度不相同的两个向量不能运算");
		for (int i = 0; i < len; i++) {
			values[i] = vector.values[i];
		}
		return this;
	}
	
	/**
	 * 标准化
	 */
	public Vector nor(){
		float lensq = lenSQ();
		
		if (lensq == 0) {
			return this;
		}
		else{
			return div(FloatMath.sqrt(lensq));
		}
	}
	/**
	 * 与另一向量相加（改变本向量，如果结果为新向量，调用静态方法add()）
	 */
	public Vector add(Vector vector){
		if(!dimensionEquals(vector)) throw new RuntimeException("Vector:维度不相同的两个向量不能运算");
		for (int i = 0; i < len; i++) {
			values[i] += vector.values[i];
		}
		
		return this;
	}
	
	public Vector add(float x, float y, float z){
		add(x, y);
		
		if(len == 3) values[2] += z;
		return this;
	}
	
	public Vector add(float x, float y){
		values[0] += x;
		values[1] += y;
		return this;
	}
	
	
	/**
	 * 与另一向量相减（改变本向量，如果结果为新向量，调用静态方法sub()）
	 */
	public Vector sub(Vector vector){
		if(!dimensionEquals(vector)) throw new RuntimeException("Vector:维度不相同的两个向量不能运算");
		for (int i = 0; i < len; i++) {
			values[i] -= vector.values[i];
		}
		
		return this;
	}
	
	public Vector sub(float x, float y, float z){
		values[0] -= x;
		values[1] -= y;
		if(len == 3) values[2] -= z;
		return this;
	}
	
	public Vector sub(float x, float y){
		values[0] -= x;
		values[1] -= y;
		return this;
	}
	
	/**
	 * 向量缩放（改变本向量）
	 */
	public Vector mul(float scale){
		for (int i = 0; i < len; i++) {
			values[i] *= scale;
		}
		return this;
	}

	/**
	 * 向量的模（长度）
	 */
	public float len(){
		return FloatMath.sqrt(lenSQ());
	}
	
	public float lenSQ(){
		float sum = 0;
		for (int i = 0; i < len; i++) {
			sum += values[i] * values[i];
		}
		return sum;
	}
	
	/**
	 * 与另一向量叉积
	 */
	public float cro2D(Vector vector){
		if(!dimensionEquals(vector)) throw new RuntimeException("Vector:维度不相同的两个向量不能运算");
		if(len == 3) throw new RuntimeException("Vector:三维向量叉积需调用cro3D()方法");
		return values[0] * vector.values[1] - vector.values[0] * values[1];
	}
	
	public Vector cro3D(Vector vector){
		if(!dimensionEquals(vector)) throw new RuntimeException("Vector:维度不相同的两个向量不能运算");
		if(len == 2) throw new RuntimeException("Vector:二维向量叉积需调用cro2D()方法");
		values[0] = values[1] * vector.values[2] - values[2] * vector.values[1];
		values[1] = values[2] * vector.values[0] - values[0] * vector.values[2];
		values[2] = values[0] * vector.values[1] - values[1] * vector.values[0];
		return this;
	}
	
	/**
	 * 与另一向量点积
	 */
	public float dot(Vector vector){
		if(!dimensionEquals(vector)) throw new RuntimeException("Vector:维度不相同的两个向量不能运算");

		float sum = 0;
		for (int i = 0; i < len; i++) {
			sum += values[i] * vector.values[i];
		}
		return sum;
	}
	
	public float distSQ(Vector vector){
		if(!dimensionEquals(vector)) throw new RuntimeException("Vector:维度不相同的两个向量不能运算");
		
		float sum = 0;
		
		for (int i = 0; i < len; i++) {
			float d = values[i] - vector.values[i];
			sum += d * d;
		}
		
		return sum;
	}
	
	public float dist(Vector vector){
		return FloatMath.sqrt(distSQ(vector));
	}
	
	public Vector div(float d){
		for (int i = 0; i < len; i++) {
			values[i] /= d;
		}
		
		return this;
	}
	
	public boolean isEquals(Vector vector){
		if(len != vector.len) return false;
		
		for (int i = 0; i < len; i++) {
			if(values[i] != vector.values[i]){
				return false;
			}
		}
		
		return true;
	}
	
	public boolean isUnit(){
		return lenSQ() == 1;
	}
	
	public boolean isZero(){
		for (int i = 0; i < len; i++) {
			if(values[i] != 0) return false;
		}
		return true;
	}
	
	public String toString(){
		if(len == 3){
			return "("+values[0]+","+values[1]+","+values[2]+")";
		}
		else{
			return "("+values[0]+","+values[1]+")";
		}
	}
	public Vector rotate(float angle){
		if(len == 2) return rotateZ(angle);
		else return rotate(angle, true, true, true);
	}
	
	public Vector rotateZ(float angle) {
		angle *= MathUtils.degreesToRadians;
		float cos = MathUtils.cos(angle);
		float sin = MathUtils.sin(angle);

		values[0] = values[0] * cos - values[1] * sin;
		values[1] = values[0] * sin + values[1] * cos;

		return this;
	}
	public Vector rotate(float angle, boolean axisX, boolean axisY, boolean axisZ){
		angle *= MathUtils.degreesToRadians;
		float cos = MathUtils.cos(angle);
		float sin = MathUtils.sin(angle);
		if(axisX){
			// rotate x;
			values[1] = values[1] * cos - values[2] * sin;
			values[2] = values[1] * sin + values[2] * cos;
		}
		if(axisY){
			// rotate y;
			values[0] = values[0] * cos + values[2] * sin;
			values[2] = values[0] * -sin + values[2] * cos;
		}
		if(axisZ){
			// rotate z;
			values[0] = values[0] * cos - values[1] * sin;
			values[1] = values[0] * sin + values[1] * cos;
		}
		return this;
	}
	public Vector rotateX(float angle){
		angle *= MathUtils.degreesToRadians;
		float cos = MathUtils.cos(angle);
		float sin = MathUtils.sin(angle);

		values[1] = (values[1] * cos) - (values[2] * sin);
		values[2] = (values[1] * sin) + (values[2] * cos);
		return this;
	}
	
	public Vector rotateY(float angle){
		angle *= MathUtils.degreesToRadians;		
		float cos = MathUtils.cos(angle);
		float sin = MathUtils.sin(angle);

		values[0] = (values[0] * cos) + (values[2] * sin);
		values[2] = (values[0] * -sin) + (values[2] * cos);
		return this;
	}
	
	public Vector copy(){
		Vector vector = new Vector(len == 2 ? Dimension.d2d : Dimension.d3d);
		System.arraycopy(values, 0, vector.values, 0, len);
		return vector;
	}
	
	public static Vector add(Vector v1, Vector v2){
		if(!v1.dimensionEquals(v2)) throw new RuntimeException("Vector:两向量维度不同，不能运算");

		Vector vector = new Vector(v1.len == 2 ? Dimension.d2d : Dimension.d3d);
		return vector.set(v1).add(v2);
	}
	
	public static Vector sub(Vector v1, Vector v2){
		if(!v1.dimensionEquals(v2)) throw new RuntimeException("Vector:两向量维度不同，不能运算");

		Vector vector = new Vector(v1.len == 2 ? Dimension.d2d : Dimension.d3d);
		return vector.set(v1).sub(v2);
	}
	
	public static float dot(Vector v1, Vector v2){
		if(!v1.dimensionEquals(v2)) throw new RuntimeException("Vector:两向量维度不同，不能运算");
		Vector vector = new Vector(v1.len == 2 ? Dimension.d2d : Dimension.d3d);
		return vector.set(v1).dot(v2);
	}
	
	public static float cro2D(Vector v1, Vector v2){
		if(!v1.dimensionEquals(v2)) throw new RuntimeException("Vector:两向量维度不同，不能运算");
		if(v1.len == 3) throw new RuntimeException("Vector:三维向量叉积需调用cro3D()方法");
		Vector vector = new Vector(Dimension.d2d);
		return vector.set(v1).cro2D(v2);
	}
	
	public static Vector cro3D(Vector v1, Vector v2){
		if(!v1.dimensionEquals(v2)) throw new RuntimeException("Vector:两向量维度不同，不能运算");
		if(v1.len == 2) throw new RuntimeException("Vector:三维向量叉积需调用cro2D()方法");
		Vector vector = new Vector(Dimension.d3d);
		return vector.set(v1).cro3D(v2);
	}
}
